﻿using System;
using System.Drawing;
using System.Threading;
using CI.Mesopotamia.Neural;
using CI.Organism;
using CI.Utils;
using Microsoft.Ccr.Core;
using Microsoft.Dss.ServiceModel.DsspServiceBase;

#region Simulation namespaces

using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
#endregion

namespace CI.MSRoboticsEnv
{
    /// <summary>
    /// Base iRobot
    /// </summary>
    public class iRobot : IPhysicalEnvironment
    {

        #region Properties
        /// <summary>
        /// Base object of the Organism in the physical environment, used to get locations, place in environment
        /// </summary>
        public object BaseObject { get { return _iRobotCreate; } }
        private IRobotCreate _iRobotCreate;

        private readonly SimEnv _simEnv = SimEnv.Instance;
        /// <summary>
        /// Organism it is relative to
        /// </summary>
        public IOrganism Organism { get; private set; }

        /// <summary>Robots bumpers</summary>
        internal BumperArrayEntity BumperArray { get; private set; }
        private SynapseIODelegate _leftBumperHandler;
        private SynapseIODelegate _rightBumperHandler;
        private bool _leftBump;
        private bool _rightBump;

        private Action<Int64> _turnLeft;
        private Action<Int64> _turnRight;
        private Action<Int64> _forward;

        // max irobot wheel max torque, when set at 15 it was capable of flipping itself
        private const float IROBOTMAXWHEELTORQUE = 12;
        private const int BUMPERHITVALUE = 100;

        // bumper sleep time waiting for initialization
        private const int MAX_INIT_WAIT_COUNT = 5;

        private WheelQueueCount _queueLeftWheel = new WheelQueueCount();
        private WheelQueueCount _queueRightWheel = new WheelQueueCount();
        private const int QUEUE_PAUSE_COUNT = 1;
        private const int QUEUE_SLEEP_TIME = 50;

        private SynapseIODelegate _cameraHandler;



        // bumper ports
        private readonly Port<EntityContactNotification> _bumperPort = new Port<EntityContactNotification>();

        public CameraEntity Camera { get; private set; }
        private readonly Port<Bitmap> _cameraPort = new Port<Bitmap>();

        #endregion

        /// <summary>
        /// ctor, create the irobot base and call the bumpers setup
        /// </summary>
        /// <param name="location"></param>
        /// <param name="organism">organism the irobot belongs too</param>
        public iRobot(ILocation location, IOrganism organism)
        {
            // use supplied entity that creates a motor base 
            // with 2 active wheels and one caster
            _iRobotCreate = new IRobotCreate(new Vector3(location.XAxis, location.YAxis, location.ZAxis));

            // specify mesh. 
            _iRobotCreate.State.Assets.Mesh = "IRobot-Create.bos";

            // the name below must match manifest
            _iRobotCreate.State.Name = "IRobot: " + organism.Guid;

            // save the composite guid
            Organism = organism;

            // create the bumpers
            CreateBumpers();

            // create camera;
            CreateCamera();

            // register to be notified on updates
            Organism.NeuralSystem.NotifyEvaluationStart += NeuralSystemNotifyEvaluationStart;

            _turnLeft = new Action<long>(TurnLeft);
            _turnRight = new Action<long>(TurnRight);
            _forward = new Action<long>(Forward);
        }

        void NeuralSystemNotifyEvaluationStart()
        {
            if ((_leftBump) && (_leftBumperHandler != null))
                _leftBumperHandler(BUMPERHITVALUE);
            if ((_rightBump) && (_rightBumperHandler != null))
                _rightBumperHandler(BUMPERHITVALUE);
        }


        /// <summary>
        /// Create bumpers for the robot
        /// </summary>
        private void CreateBumpers()
        {

            if (_iRobotCreate.Bumpers == null)
            {
                var bumperName = new[] { "L1", "L2", "L3", "B1", "B2", "R1", "R2", "R3" };
                // add some bumper shapes
                const float start = (float)(-0.78 * Math.PI / 2.0f);
                const float end = (float)(0.78 * Math.PI / 2.0f);
                const int segments = 8;
                const float increment = (end - start) / (segments - 1);
                var segment = 0;
                var bumpers = new BoxShape[segments];
                for (float angle = start; angle <= end; angle += increment, segment++)
                {
                    var bumperBox = new BoxShape(
                        new BoxShapeProperties(
                        bumperName[segment], 0.001f, //mass
                        new Pose(new Vector3((float)(0.16 * Math.Sin(angle)), 0.055f, (float)(-0.16 * Math.Cos(angle))), // position
                        Quaternion.FromAxisAngle(0, 1, 0, angle)), // rotation
                        new Vector3(0.053f, 0.06f, 0.01f))); // dimensions

                    bumperBox.BoxState.Material = new MaterialProperties("iRobotCreate chassis", 0.0f, 0.25f, 0.5f);
                    bumperBox.Parent = _iRobotCreate;
                    bumperBox.BoxState.EnableContactNotifications = true;
                    if (bumperName[segment][0] == 'L')
                        bumperBox.State.DiffuseColor.X = 1.0f;
                    else if (bumperName[segment][0] == 'R')
                        bumperBox.State.DiffuseColor.Y = 1.0f;
                    else if (bumperName[segment][0] == 'B')
                    {
                        bumperBox.State.DiffuseColor.X = 1.0f;
                        bumperBox.State.DiffuseColor.Y = 1.0f;
                    }
                    bumpers[segment] = bumperBox;
                }
                _iRobotCreate.Bumpers = bumpers;
            }


            // create bumper array
            BumperArray = new BumperArrayEntity(_iRobotCreate.Bumpers)
                              {
                                  State = { Name = "Bumpers: " + Organism.Guid }
                              };

            _iRobotCreate.InsertEntity(BumperArray);
        }

        /// <summary>
        /// Fires the bumper handler if attached
        /// </summary>
        /// <param name="contact"></param>
        private void BumperHandler(EntityContactNotification contact)
        {
            if (_iRobotCreate == null || _iRobotCreate.SuspendUpdates)
                return;

            switch (contact.Stage)
            {
                case ContactNotificationStage.Started:
                    UpdateAndActivateBumperHandlers(contact, true);
                    break;
                case ContactNotificationStage.Finished:
                    UpdateAndActivateBumperHandlers(contact, false);
                    break;
            }
        }

        private void UpdateAndActivateBumperHandlers(EntityContactNotification contact, bool bumperState)
        {
            foreach (var shapeContact in contact.ShapeContacts)
            {
                _simEnv.TraceLogger.LogDebug(string.Format("Bumper Hit entity {0} on bumper {1} or organism {2}", contact.Stage, shapeContact.LocalShape.State.Name.Substring(shapeContact.LocalShape.State.Name.Length - 2), Organism.Guid));

                var bumper = shapeContact.LocalShape.State.Name.Substring(shapeContact.LocalShape.State.Name.Length - 2, 1);

                // if left bumper
                if (bumper == "L")
                {
                    _leftBump = bumperState;
                    if ((_leftBump) && (_leftBumperHandler != null))
                        _leftBumperHandler(BUMPERHITVALUE);
                }

                // if right bumper
                if (bumper == "R")
                {
                    _rightBump = bumperState;
                    if ((_rightBump) && (_rightBumperHandler != null))
                        _rightBumperHandler(BUMPERHITVALUE);
                }
            }
        }

        /// <summary>
        /// Bind all physical world events to SynapseIO
        /// eg "Left Bumper" to a IO synapse, "Right Drive" to a IO synapse
        /// </summary>
        public void BindSynapseIO()
        {
            CI.Organism.iRobot.BindSynapseIO(Organism, ref _leftBumperHandler, ref _rightBumperHandler, ref _cameraHandler, ref _turnLeft, ref _turnRight, ref _forward);
        }

        /// <summary>
        /// Unbind all physical world events to SynapseIO
        /// eg "Left Bumper" to a IO synapse, "Right Drive" to a IO synapse
        /// </summary>
        public void UnBindSynapseIO()
        {
            CI.Organism.iRobot.UnbindSynapseIO(Organism, ref _leftBumperHandler, ref _rightBumperHandler, ref _cameraHandler, ref _turnLeft, ref _turnRight, ref _forward);
        }

        /// <summary>
        /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources.
        /// </summary>
        public void Dispose()
        {
            Dispose(true);
            GC.SuppressFinalize(this);
        }

        private bool _disposed = false;

        protected virtual void Dispose(bool disposing)
        {
            if (_disposed)
                return;

            _disposed = true;

            if (disposing)
            {
                // get rid of managed resources 
            }

            Organism.NeuralSystem.NotifyEvaluationStart -= NeuralSystemNotifyEvaluationStart;
            UnBindSynapseIO();
            _turnLeft = null;
            _turnRight = null;
            _forward = null;
            _leftBumperHandler = null;
            _rightBumperHandler = null;
            _queueLeftWheel = null;
            _queueRightWheel = null;
            Organism = null;

            BumperArray = null;
            Camera = null;
            _iRobotCreate = null;
        }

        ~iRobot()
        {
            Dispose(false);
        }


        private void TurnLeft(Int64 outputValue)
        {
            OutputBoundServices(outputValue, Globals.IROBOTTURNLEFT);
        }

        private void TurnRight(Int64 outputValue)
        {
            OutputBoundServices(outputValue, Globals.IROBOTTURNRIGHT);
        }

        private void Forward(Int64 outputValue)
        {
            OutputBoundServices(outputValue, Globals.IROBOTFORWARD);
        }

        /// <summary>
        /// Process all the output services, check their name in the binding name then call their function
        /// </summary>
        /// <param name="outputValue">output value to use</param>
        /// <param name="bindingName">binding name</param>
        private void OutputBoundServices(Int64 outputValue, string bindingName)
        {
            if (_iRobotCreate.SuspendUpdates)
                return;

            _simEnv.TraceLogger.LogDebug(string.Format("Executing on output value of {0} on service {1}", outputValue, bindingName));

            // if it is above the max then use the max
            if (outputValue > IROBOTMAXWHEELTORQUE)
                outputValue = (long)IROBOTMAXWHEELTORQUE;

            switch (bindingName)
            {
                case Globals.IROBOTTURNLEFT:
                    // queue the drive wheel
                    QueueDriveWheel(_iRobotCreate.LeftWheel.Wheel, outputValue, _queueLeftWheel);
                    QueueDriveWheel(_iRobotCreate.RightWheel.Wheel, outputValue * -1, _queueRightWheel);
                    break;

                case Globals.IROBOTTURNRIGHT:
                    // queue the drive wheel
                    QueueDriveWheel(_iRobotCreate.LeftWheel.Wheel, outputValue * -1, _queueLeftWheel);
                    QueueDriveWheel(_iRobotCreate.RightWheel.Wheel, outputValue, _queueRightWheel);
                    break;

                case Globals.IROBOTFORWARD:
                    // queue the drive wheel
                    QueueDriveWheel(_iRobotCreate.LeftWheel.Wheel, outputValue * -1, _queueLeftWheel);
                    QueueDriveWheel(_iRobotCreate.RightWheel.Wheel, outputValue * -1, _queueRightWheel);
                    break;
            }

        }

        /// <summary>
        /// Queue the drive wheel
        /// </summary>
        /// <param name="wheel">wheel to queue</param>
        /// <param name="axelSpeed">speed</param>
        /// <param name="wheelQueue"></param>
        void QueueDriveWheel(PhysicsWheel wheel, float axelSpeed, WheelQueueCount wheelQueue)
        {

            _simEnv.TraceLogger.LogDebug(string.Format("Queuing wheel for organism {0} with Queue Count {1}", Organism.Guid, wheelQueue.Count));

            // if the queue is high then sleep for a big
            while (wheelQueue.Count >= QUEUE_PAUSE_COUNT)
            {
                _simEnv.TraceLogger.LogDebug(string.Format("Queuing wheel for organism {0} Queue Count {1} with Last AxelSpeed {2} and new AxelSpeed", Organism.Guid, wheelQueue.Count, wheelQueue.LastAxelSpeed, axelSpeed));

                if (wheelQueue.LastAxelSpeed == axelSpeed)
                    return;

                _simEnv.TraceLogger.LogDebug(string.Format("Queuing wheel for organism {0} with Queue Count {1} high and sleeping", Organism.Guid, wheelQueue.Count));
                StatsLogger.Sleeper(QUEUE_SLEEP_TIME);
            }

            wheelQueue.LastAxelSpeed = axelSpeed;
            Interlocked.Increment(ref wheelQueue.Count);
            var deferredTask = new Task<PhysicsWheel, float, WheelQueueCount>(wheel, axelSpeed, wheelQueue, DriveWheel);
            _iRobotCreate.DeferredTaskQueue.Post(deferredTask);

        }

        /// <summary>
        /// Drive a partiucal wheel at particular speed
        /// </summary>
        /// <param name="wheel">wheel to drive</param>
        /// <param name="axelSpeed">axel speed to set</param>
        /// <param name="wheelQueue"></param>
        void DriveWheel(PhysicsWheel wheel, float axelSpeed, WheelQueueCount wheelQueue)
        {
            _simEnv.TraceLogger.LogDebug(string.Format("Driving wheel for organism {0} with Queue Count {1} with Axel speed {2}", Organism.Guid, wheelQueue.Count, axelSpeed));
            wheel.AxleSpeed = axelSpeed;
            Interlocked.Decrement(ref wheelQueue.Count);
        }

        /// <summary>
        /// Subscribe the bumpers to the listening port
        /// </summary>
        public void SubscribeBumbers(DsspServiceBase dsspServiceBase)
        {
            if (!IsEntityInitialized(BumperArray))
                return;

            try
            {
                // subscribe to bumps            
                BumperArray.Subscribe(_bumperPort);

                // set the bumper port to direct to the handler
                dsspServiceBase.Activate(Arbiter.Receive(true, _bumperPort, BumperHandler));
            }
            catch (Exception exception)
            {
                // have seen strange null reference errors in a native code called by BumperArray.Subscribe, this should handle it
                const string restartMsg = "Error Creating bumpers";
                _simEnv.TraceLogger.LogException(restartMsg, exception);
                SimEnv.Instance.TriggerScenarioRestart(restartMsg);
            }
        }

        private bool IsEntityInitialized(VisualEntity visualEntity)
        {
            var maxWaitCount = 0;
            var initWaitTime = 10;

            // wait for bumper to be ready, sleep for 100 miliseconds
            while (!visualEntity.HasBeenInitialized)
            {
                // increase by a factor of 10 each time
                initWaitTime *= 10;
                _simEnv.TraceLogger.LogWarn(visualEntity.State.Name + " : not initialized sleeping and trying again in:" + initWaitTime + " on org with guid: " + Organism.Guid);

                // make sure a break hasnt been set
                if (SimEnv.Instance.BreakExit)
                {
                    return false;
                }

                // make sure we dont wait forever
                maxWaitCount++;
                if (maxWaitCount == MAX_INIT_WAIT_COUNT)
                {
                    // error subscribing to service
                    throw new iRobotException("Unable to init : " + visualEntity.State.Name);
                }

                StatsLogger.Sleeper(initWaitTime);
                SimEnv.Instance.HostEnvPhysical.WaitForIdle();
            }

            return true;
        }

        private void CreateCamera()
        {
            // low resolution, wide Field of View
            const float viewAngle = ((float)Math.PI * 0.4f);
            Camera = new CameraEntity(50, 50, viewAngle, CameraEntity.CameraModelType.AttachedChild)
                         {
                             State =
                                 {
                                     Name = "Camera : " + Organism.Guid,
                                     Pose = { Position = new Vector3(0.0f, 0.2f, 0.0f) }
                                 },
                         };

            //Tilt the camera downwards
            const float angle = (float)(-15 * Math.PI / 180);
            var axisAngle = new AxisAngle(new Vector3(1, 0, 0), angle);
            Camera.State.Pose.Orientation = Quaternion.FromAxisAngle(axisAngle);

            // camera renders in offline buffer at each frame
            Camera.IsRealTimeCamera = true;
            Camera.UpdateInterval = 2000;
            _iRobotCreate.InsertEntity(Camera);
        }


        public void SubscribeCamera(DsspServiceBase dsspServiceBase)
        {
            if (!IsEntityInitialized(Camera))
                return;
            try
            {
                Camera.Subscribe(_cameraPort);

                var visualSynapseIO = (VisualSynapseIO)Organism.GetSynapseIO(Globals.CYCLOPS);
                if (visualSynapseIO.VS == null)
                    visualSynapseIO.VS = new SimpleVisionSystem(visualSynapseIO.FormatType);

                // set the bumper port to direct to the handler
                dsspServiceBase.Activate(Arbiter.Receive(true, _cameraPort, ((VisualSynapseIO)Organism.GetSynapseIO(Globals.CYCLOPS)).VS.ProcessNewBitmap));
            }
            catch (Exception exception)
            {
                // have seen strange null reference errors in a native code called by BumperArray.Subscribe, this should handle it
                const string restartMsg = "Error Creating Camera";
                _simEnv.TraceLogger.LogException(restartMsg, exception);
                SimEnv.Instance.TriggerScenarioRestart(restartMsg);
            }
        }



        /// <summary>
        /// iRobot exceptions
        /// </summary>
        public class iRobotException : SystemException
        {
            /// <summary>
            /// iRobot exception with a message describing it
            /// </summary>
            /// <param name="message"></param>
            public iRobotException(string message) : base(message) { }
        }

    }

    /// <summary>
    /// Wheel Queue count
    /// </summary>
    public class WheelQueueCount
    {
        public int Count;
        public float LastAxelSpeed;
    }
}
